/* -------------------------------------------------------------------------------
----------------------------------------------------------------------------------
RNE
----------------------------------------------------------------------------------
-------------------------------------------------------------------------------- */
#include <math.h>
#include <string.h>
#define M_PI 3.14159265358979323846


    const double alpha_ij [] = {M_PI/2, 0.0, M_PI/2, M_PI/2, M_PI/2, 0.0};
    const double a_ij [] = {0.0, 0.45, 0.0, 0.0, 0.0, 0.0};
    const double S_ij [] = {0.0, 0.0, 0.0, 0.48, 0.0, 0.07};
    const double offset [] = {M_PI/2, M_PI/2, M_PI/2, M_PI, M_PI, 0.0};
    const double mass_ij [] = {9.29, 12.43, 4.86, 3.08, 2.07, 1.05};
    double r_com[6][3] = { {0, 0.117, 0},
                           {-0.321, -0.030, 0.0},
                           {0.0, 0.0, 0.0485},
                           {0.0, 0.1122, 0.0},
                           {0.0, 0.0, -0.042},
                           {0.0, 0.0, -0.048}};
    double I_vec[6][6] = {  {0.13075780000,	 0.10682180000,	 0.05280030000,	 -0.00000001000,	 -0.00000002000,  -0.00000001000},
                            {0.06409580000,	 0.37828040000,	 0.37582010000,	 -0.02012530000,	 -0.00029530000,  -0.00087560000},
                            {0.05985720000,    0.00837430000,	 0.05773400000,   -0.00000002000,	 0.00181740000,	 0.00000002000},
                            {0.02826870000,    0.01993750000,   0.01090200000,   0.00000000000,	 0.00000000000,	 0.00000001000},
                            {0.00741330000,    0.00301890000,   0.00698520000,   -0.00001649000,	 -0.00000000000,	 -0.00000000000},
                            {0.00135040000,    0.00135040000,   0.00058330000,   0.00000000000,	 0.00000000000,	 0.00000000000} };
    double z [] = {0, 0 , 1};
    const double gearRatio = 50;

int Cross(double a[], double b[], double c[]) {

c[0] = a[1]*b[2] - a[2]*b[1];
c[1] = -1*a[0]*b[2] + a[2]*b[0];
c[2] = a[0]*b[1] - a[1]*b[0];


return 0;
}

int MatrixMult(double a[][3], double b[], double c[]) {
double sum=0;
int ii,ll;

        for (ii=0; ii<3; ii++) {
                sum = 0;
                for (ll=0; ll<3; ll++) {sum += a[ii][ll]*b[ll];}
                c[ii] = sum;
        }

return 0;
}

int InertiaMult(double a[], double b[], double c[]) {
c[0] = a[0]*b[0] + a[3]*b[1] + a[5]*b[2];
c[1] = a[3]*b[0] + a[1]*b[1] + a[4]*b[2];
c[2] = a[5]*b[0] + a[4]*b[1] + a[2]*b[2];

return 0;
}

int Transpose(double a[][3], double b[][3]) {
b[0][0] = a[0][0];
b[0][1] = a[1][0];
b[0][2] = a[2][0];

b[1][0] = a[0][1];
b[1][1] = a[1][1];
b[1][2] = a[2][1];

b[2][0] = a[0][2];
b[2][1] = a[1][2];
b[2][2] = a[2][2];

return 0;
}

/* ---<>------------------------------------------------------------------
                        Start of Main Program
----------------------------------------------------------------------- */
extern "C" int RNELite(const double q[], double tau[][6], int choice) {
int ii, pp, outerLoops;
double gravity;

if (choice == 0 ) {
    //Inertia Matrix
    outerLoops = 6;
    gravity = 0.0;
}
else{
    // Gravity Torque
    outerLoops = 1;
    gravity = 9.81;

}

double a,alpha,thetaA,S;

double qdd[] = {0,0,0,0,0,0};
/* ---<>--------------------------------
    Outer Loop for inertia
----------------------------------------- */
for (pp=0; pp<outerLoops; pp++) {
if (outerLoops > 1) {
    qdd[pp] =  1;
    if (pp>0) { qdd[pp-1] = 0; }
}


double wd[3] = {0, 0, 0};
double w[3] = {0, 0, 0};

double vd_1[3] = {0, 0, 0};
double vd_2[3] = {0, 0, 0};
double vd_3[3] = {0, 0, 0};
double vd[3] = {0, 0, gravity};

double vhat_1[3] = {0.0, 0.0, 0.0};
double vhat_2[3] =  {0.0, 0.0, 0.0};
double vhat[3] =  {0.0, 0.0, 0.0};

double F[3][6] = {0.0};
double N[3][6] = {0.0};

/* ---<>--------------------------------
Forward Recursion
----------------------------------------- */
    for (ii=0; ii<6; ii++) {
                double temp_a[3] = {0};
                a = a_ij[ii];
                alpha = alpha_ij[ii];
                thetaA = q[ii]+offset[ii];
                S = S_ij[ii];
                double R_t[3][3] = {0};

                double R[3][3] =  {   {cos(thetaA), -sin(thetaA)*cos(alpha), sin(thetaA)*sin(alpha) },
                                {sin(thetaA), cos(thetaA)*cos(alpha), -cos(thetaA)*sin(alpha) },
                                {0.0, sin(alpha), cos(alpha)} };

                Transpose(R,R_t);

                double pstar[3] = {a, S*sin(alpha), S*cos(alpha)};
                double add[3] = {wd[0]+z[0]*qdd[ii], wd[1]+z[1]*qdd[ii], wd[2]+z[2]*qdd[ii]};

                MatrixMult(R_t,add,wd);
                MatrixMult(R_t,w,w);

                Cross(wd,pstar,vd_1);
                Cross(w,pstar,temp_a);
                Cross(w,temp_a,vd_2);
                MatrixMult(R_t,vd,vd_3);
                vd[0] = vd_1[0]+vd_2[0]+vd_3[0];
                vd[1] = vd_1[1]+vd_2[1]+vd_3[1];
                vd[2] = vd_1[2]+vd_2[2]+vd_3[2];

                Cross(wd,r_com[ii],vhat_1);
                Cross(w,r_com[ii],temp_a);
                Cross(w,temp_a,vhat_2);
                vhat[0] = vhat_1[0]+vhat_2[0]+vd[0];
                vhat[1] = vhat_1[1]+vhat_2[1]+vd[1];
                vhat[2] = vhat_1[2]+vhat_2[2]+vd[2];

                F[0][ii] = mass_ij[ii]*vhat[0];
                F[1][ii] = mass_ij[ii]*vhat[1];
                F[2][ii] = mass_ij[ii]*vhat[2];

                InertiaMult(I_vec[ii],wd,temp_a);
                N[0][ii] = temp_a[0];
                N[1][ii] = temp_a[1];
                N[2][ii] = temp_a[2];
    }

/* ---<>--------------------------------
Backwards Recursion
----------------------------------------- */
double f[3] = {0};
double nm[3] = {0};
 for (ii=5; ii>-1; ii--) {
                double R[3][3] = {0};
                double R_t[3][3] = {0};

                double temp_1[3] = {0};
                double temp_2[3] = {0};

                double nm_1[3] = {0};
                double nm_2[3] = {0};

                a = a_ij[ii];
                alpha = alpha_ij[ii];
                S = S_ij[ii];
                double pstar[3] = {a, S*sin(alpha), S*cos(alpha)};

                if (ii==5) {
                     double Ra[3][3] = { {1, 0, 0},
                                {0, 1, 0},
                                {0, 0, 1} };
                    memcpy(R, Ra, sizeof(Ra));
                }
                else {
                    alpha = alpha_ij[ii+1];
                    thetaA = q[ii+1]+offset[ii+1];
                    double Ra[3][3] =  {   {cos(thetaA), -sin(thetaA)*cos(alpha), sin(thetaA)*sin(alpha) },
                                {sin(thetaA), cos(thetaA)*cos(alpha), -cos(thetaA)*sin(alpha) },
                                {0.0, sin(alpha), cos(alpha)} };
                    memcpy(R, Ra, sizeof(Ra));
                }

                Transpose(R,R_t);

                MatrixMult(R_t,pstar,temp_1);
                Cross(temp_1,f,temp_2);

                temp_2[0] = nm[0] + temp_2[0];
                temp_2[1] = nm[1] + temp_2[1];
                temp_2[2] = nm[2] + temp_2[2];

                MatrixMult(R,temp_2,nm_1);

                temp_1[0] = pstar[0] + r_com[ii][0];
                temp_1[1] = pstar[1] + r_com[ii][1];
                temp_1[2] = pstar[2] + r_com[ii][2];

                temp_2[0] = F[0][ii];
                temp_2[1] = F[1][ii];
                temp_2[2] = F[2][ii];
                Cross(temp_1,temp_2,nm_2);
                nm[0] = nm_1[0] + nm_2[0] + N[0][ii];
                nm[1] = nm_1[1] + nm_2[1] + N[1][ii];
                nm[2] = nm_1[2] + nm_2[2] + N[2][ii];

                MatrixMult(R,f,temp_1);
                f[0] = temp_1[0] + F[0][ii];
                f[1] = temp_1[1] + F[1][ii];
                f[2] = temp_1[2] + F[2][ii];

                alpha = alpha_ij[ii];
                thetaA = q[ii]+offset[ii];
                double Rb[3][3] =  {   {cos(thetaA), -sin(thetaA)*cos(alpha), sin(thetaA)*sin(alpha) },
                                {sin(thetaA), cos(thetaA)*cos(alpha), -cos(thetaA)*sin(alpha) },
                                {0.0, sin(alpha), cos(alpha)} };
                Transpose(Rb,R_t);
                MatrixMult(R_t,z,temp_1);
                tau[pp][ii] = nm[0]*temp_1[0] + nm[1]*temp_1[1] + nm[2]*temp_1[2];
  }

 }

return 0;
}

